#include "oneservo.h"
#include <QDebug>

OneServo::OneServo()
{
}

void OneServo::setParameters(QList<double> params)
{
    angleMin = params[0];
    angleMax = params[1];
    length = params[2];
}

void OneServo::addMotor(int freq, double delay, double tau, double imprecision)
{
    motors.append(new Servomotor(this, motors.size(), freq, delay, tau, imprecision));
    motors.back()->start();

}

void OneServo::addSensor(int freq, double delay, double imprecision)
{
    sensors.append(new MotorStateSensor(this, sensors.size(), freq, delay, imprecision));
}

void OneServo::checkValue(int id)
{
    double angle = motors[id]->getState();

    if(angle > angleMax)
    {
        motors[id]->setState(angleMax);
    }
    else if(angle < angleMin)
    {
        motors[id]->setState(angleMin);
    }
}

QList<double> OneServo::getPosition()
{
    QList<double> result;

    double angleRad = (M_PI/180.)*motors[0]->getState();

    double x = length*cos(angleRad), y = length*sin(angleRad);

    result << x << y;

    return(result);
}

QList<double> OneServo::getPositionNormee()
{
     QList<double> result;

    double angleRad = (M_PI/180.)*motors[0]->getState();

    double x = (1+cos(angleRad))/2, y = (1+sin(angleRad))/2;


    result << x << y;

    return(result);
}

double OneServo::getMotorStateNormee(int id)
{
    return  (motors[id]->getState()-angleMin)/(angleMax-angleMin);
}


void OneServo::setInState(QList<double> state){
    motors[0]->setState(angleMin + state[0]*(angleMax-angleMin));
}
